#Creator: Sander Nugteren, Erik van Egmond, Auke Wiggers
#Mijn ideeen over communicatie:
#Threadable module, die structs/pickle/json kan senden en ontvangen
#elke nao stuurt hoe ver de bal van hem af is binnen een bepaalde interval
#(niet elke nao hoeft te reporten)
#de keeper beslist wie het dichtst bij de bal is, en dus er heen moet gaan
#de rest doet?
#Als de keeper de bal ziet, dan terug lopen?
#Anders vooruit lopen?

# Notulen
# - Alle data in keeper, naos lezen uit. Reden: Keeper valt uit dan geen problemen met oude waarden. 

import socket
import sys
import struct
import time
import threading
from naoqi import ALProxy

memory = ALProxy('ALMemory', '127.0.0.1', 9559)

#we first insert some data that coach needs to know about every Nao
#prefix everything with DNT, to have unique names
naoVars = ['dntState', 'dntPhase', 'dntBallDist', 'dntNaoNum']
#state is the gamestate (and also if the nao is penalized?)
#ballDist is the distance to the ball
#phase is what the nao is currently doing (going for the ball, attacking, defending)

class Coach(threading.Thread):
#de coach bewaard de output van de coaching, als wie er mag lopen
#de andere nao's bewaren de locale variablen, als baldistance
#
#de coach onthoud alleen welke nao het dichtst bij de bal is en dus naar de bal mag lopen.
#de nao's kijken bij de coach of ze mogen lopen
#als de keeper (op wie dit programma zal runnen) de bal heeft gezien
#dan moet iedereen naar het eigen doel proberen te komen
#anders moet iedereen in de aanval
#TODO: als er nog een actie bezig is om de bal te gaan onderscheppen, pass
#Als er een Nao bezig is met naar de bal gaan moet die dat constant uitzenden
#Als hij de bal schopt stopt hij daarmee. En dan moet er weer een nieuwe cycle beginnen van zoeken naar de bal
#Het programma moet dan een message sturen aan de rest om te stoppen met lopen en een bal te gaan zoeken
#TODO: AlMemory dingen bekijken om data te exchangen
# http://users.aldebaran-robotics.com/docs/site_en/bluedoc/ALMemory.html
    
    def __init__(self, name, ipList):
        threading.Thread.__init__(self)
        self.name = name
        #data the keeper should display to the other nao's
        memory.insertListData([['dnt1', '', 0], ['dnt2', '', 0], ['dnt3', '', 0], ['dnt4', '', 0]])
        #make a proxy dict containing proxys of all the other nao's
        self.proxyDict = self.makeProxys(ipList)

    def run(self):
        #Keep listening for some time.
        #If no field player has reported that it found a ball, keep listening until one does 
        while True:
            closestNao = 0
            minDist = 9999
            keeperSawBall = False
            
            messageIn = self.receiveMessage()
            #ga er hier vanuit dat we een python dict/JSON krijgen
            ballSeen = list()
            for ip in self.proxyDict: 
                currentDist = messageIn[ip]['dntBallDist']
                currentNao = messageIn[ip]['dntNaoNum']
                
                # track naos that have seen the ball
                if currentDist:
                    ballSeen.append( currentNao )
                    if currentNao == 1:
                        keeperSawBall = True
                
                    # update closest nao
                    if currentDist < minDist:
                        minDist = currentDist
                        closestNao = currentNao
            print 'Saw ball: ', ballSeen, 'Closest: ' ,closestNao           
            messageOut = list()
            for n in range(1,5):
                action = ''
                # if keeper, dont alter actions
                if n == 1:
                    action = ''
                # else if player which is not the closest nao, standby
                elif closestNao != n and n in ballSeen:
                    if keeperSawBall:
                        #action = 'Retreat'
                        action = 'Standby'
                    else:
                        action = 'Standby'
                # all other cases, proceed as usual        
                messageOut.append( ['dnt' + str(n), action, 0] )
            self.sendMessage(messageOut)
            time.sleep(1)
        
    #should recieve messages from the server
    def receiveMessage(self):
        message = {}
        for ip in self.proxyDict:
            proxy = self.proxyDict[ip]            
            message[ip]={}
            for var in naoVars:
                message[ip][var] = proxy.getData(var,0)
            
        return message

    #should send messages to everyone (broadcast?), current approach: store into memory of each nao using proxies
    def sendMessage(self, message):
        memory.insertListData(message)  # message = [['dntGetBall', <robotnumber>/''],['dntKeeperSawBall', False/True]] 
        return True
       
    def makeProxys(self, ipList):
        proxyDict = {}
        for ip in ipList:
            proxyDict[ip] = ALProxy('ALMemory', ip, 9559)
        return proxyDict
        

